#include "map_download_action_server.h"
#include <QJsonDocument>
#include <QJsonParseError>
#include <QJsonObject>
#include <QDir>
#include "../include/seer/SCHeadData.h"
MapDownloadActionServer::MapDownloadActionServer(const rclcpp::NodeOptions & options)
    : QObject()
    , rclcpp::Node("map_download_action_server", options)
    , m_confSocket(nullptr)
    , m_requestManager(nullptr)
    , m_reconnectTimer(nullptr)
    , m_wasConnected(false)
    , m_reconnectAttempts(0)
{
    // 初始化日志记录器
    logger_ = GLOBAL_LOGGER;
    LOG_INFO(" MapDownloadActionServer 初始化开始");

    // 初始化TCP连接和请求管理器
    m_confSocket = new SCStatusTcp(this);
    m_requestManager = new RequestManager(this);
    
    // 声明并获取 robot_ip 参数
    this->declare_parameter<std::string>("robot_ip", "192.168.1.100");
    std::string robot_ip_str;
    this->get_parameter("robot_ip", robot_ip_str);
    m_robotIp = QString::fromStdString(robot_ip_str);

    // 声明并获取 conf_port 参数
    this->declare_parameter<int>("conf_port", SEER_API_PORT_CONF);
    this->get_parameter("conf_port", m_confPort);
        
    LOG_INFO(" 获取参数完成: robot_ip={}, conf_port={}", 
                 m_robotIp.toStdString(), m_confPort);
    
    // 初始化连接相关组件
    initializeConnections();
    
    // 连接信号槽
    connect(m_confSocket->tcpSocket(), &QTcpSocket::stateChanged, 
            this, &MapDownloadActionServer::handleSocketStateChanged);
    connect(m_confSocket->tcpSocket(), &QTcpSocket::errorOccurred, 
            this, &MapDownloadActionServer::handleSocketError);
    
    // 创建Action服务器
    action_server_ = rclcpp_action::create_server<MapDownload>(
        this,
        "map_download",
        std::bind(&MapDownloadActionServer::handle_goal, this, std::placeholders::_1, std::placeholders::_2),
        std::bind(&MapDownloadActionServer::handle_cancel, this, std::placeholders::_1),
        std::bind(&MapDownloadActionServer::handle_accepted, this, std::placeholders::_1));

    LOG_INFO(" 地图下载Action服务器已启动");
}

rclcpp_action::GoalResponse MapDownloadActionServer::handle_goal(
    const rclcpp_action::GoalUUID & uuid,
    std::shared_ptr<const MapDownload::Goal> goal)
{
    Q_UNUSED(uuid);
    
    LOG_DEBUG(" 收到地图下载请求: {}", goal->map_name);
    
    if (goal->map_name.empty()) {
        LOG_WARN(" 地图名称为空");
        return rclcpp_action::GoalResponse::REJECT;
    }
    
    if (!isConnected()) {
        LOG_WARN(" 机器人未连接");
        return rclcpp_action::GoalResponse::REJECT;
    }
    
    return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
}

rclcpp_action::CancelResponse MapDownloadActionServer::handle_cancel(
    const std::shared_ptr<GoalHandleMapDownload> goal_handle)
{
    LOG_INFO(" 收到地图下载取消请求");
    Q_UNUSED(goal_handle);
    return rclcpp_action::CancelResponse::ACCEPT;
}

void MapDownloadActionServer::handle_accepted(
    const std::shared_ptr<GoalHandleMapDownload> goal_handle)
{
    // 使用自定义事件在主线程中执行目标，避免线程问题
    QCoreApplication::postEvent(this, new ExecuteGoalEvent(goal_handle));
}

bool MapDownloadActionServer::event(QEvent* event) {
    if (event->type() == ExecuteGoalEvent::Type) {
        auto* execute_event = static_cast<ExecuteGoalEvent*>(event);
        execute(execute_event->goal_handle());
        return true;
    }
    return QObject::event(event);
}


void MapDownloadActionServer::execute(
    const std::shared_ptr<GoalHandleMapDownload> goal_handle)
{
    LOG_INFO(" 开始执行地图下载任务");
    
    auto goal = goal_handle->get_goal();
    auto feedback = std::make_shared<MapDownload::Feedback>();
    auto result = std::make_shared<MapDownload::Result>();
    
    // 检查连接状态
    if (!isConnected()) {
        result->success = false;
        result->message = "Not connected to robot";
        LOG_ERROR(" 机器人未连接");
        goal_handle->succeed(result);
        return;
    }
    
    // 初始化统计数据
    quint32 total_chunks = 0;
    quint32 chunks_sent = 0;
    
    // 创建地图目录
    QString mapDirPath = QString("data/map");
    QDir mapDir(mapDirPath);
    if (!mapDir.exists()) {
        if (mapDir.mkpath(".")) {
            LOG_DEBUG(" 创建地图目录成功 - 路径: {}", mapDirPath.toStdString());
        } else {
            LOG_WARN(" 创建地图目录失败 - 路径: {}", mapDirPath.toStdString());
        }
    }
    
    // 准备文件写入
    QString mapFileName = QString::fromStdString(goal->map_name);
    if (!mapFileName.endsWith(".smap")) {
        mapFileName += ".smap";
    }
    QString mapFilePath = QString("%1/%2").arg(mapDirPath, mapFileName);
    
    // 使用信号槽机制处理下载，避免线程问题
    QString mapName = QString::fromStdString(goal->map_name);
    auto download_context = new MapDownloadContext{
        goal_handle,
        mapFilePath,
        chunks_sent,
        total_chunks,
        result,
        feedback
    };
    
    // 使用信号槽连接下载完成事件
    QObject::connect(this, &MapDownloadActionServer::downloadFinished, this, 
        [this, download_context](MapDownloadContext* context) {
            Q_UNUSED(context);
            // 删除这部分会清空文件的代码
            /*
            QFile mapFile(download_context->file_path);
            if (mapFile.open(QIODevice::WriteOnly)) {
                mapFile.close(); // 只是为了确保文件存在
            }
            */
            
            download_context->result->success = true;
            download_context->result->message = "Download completed successfully";
            download_context->result->total_chunks = download_context->chunks_sent;
            download_context->goal_handle->succeed(download_context->result);
            LOG_INFO(" 地图下载完成 - 总块数: {}", download_context->chunks_sent);
            delete download_context;
        }, Qt::QueuedConnection);
    
    QObject::connect(this, &MapDownloadActionServer::downloadFailed, this,
        [this, download_context](MapDownloadContext* context, const QString& error_msg) {
            Q_UNUSED(context);
            QFile mapFile(download_context->file_path);
            if (mapFile.open(QIODevice::WriteOnly)) {
                mapFile.close(); // 只是为了确保文件存在
            }
            
            download_context->result->success = false;
            download_context->result->message = error_msg.toStdString();
            download_context->result->total_chunks = download_context->chunks_sent;
            download_context->goal_handle->succeed(download_context->result);
            LOG_ERROR(" 地图下载失败: {}", error_msg.toStdString());
            delete download_context;
        }, Qt::QueuedConnection);
    
    QObject::connect(this, &MapDownloadActionServer::downloadCanceled, this,
        [this, download_context](MapDownloadContext* context) {
            Q_UNUSED(context);
            QFile mapFile(download_context->file_path);
            if (mapFile.open(QIODevice::WriteOnly)) {
                mapFile.close(); // 只是为了确保文件存在
            }
            
            download_context->result->success = false;
            download_context->result->message = "Download canceled";
            download_context->result->total_chunks = download_context->chunks_sent;
            download_context->goal_handle->canceled(download_context->result);
            LOG_INFO(" 地图下载被取消");
            delete download_context;
        }, Qt::QueuedConnection);

    // 开始下载地图数据
    downloadMapStreaming(mapName, [this, download_context](const QByteArray &data, bool is_last_chunk) {
    // 检查是否取消
        if (download_context->goal_handle->is_canceling()) {
            Q_EMIT downloadCanceled(download_context);
            return;
        }
        
        if (data.isEmpty() && !is_last_chunk) {
            // 下载失败
            Q_EMIT downloadFailed(download_context, "Download failed - empty data");
            return;
        }
        
        // 处理数据块，不写入文件，只发送给客户端
        if (!data.isEmpty()) {
            // 分块传输到客户端
            const size_t CHUNK_SIZE = 262144; // 256KB per chunk (262144 bytes = 2^18)
            size_t total_size = data.size();
            size_t sent_bytes = 0;
            
            // 计算当前数据的总块数
            size_t current_data_chunks = (total_size + CHUNK_SIZE - 1) / CHUNK_SIZE;
            
            // 更新总块数
            download_context->total_chunks = current_data_chunks;
            
            // 分块发送数据 - 修复：确保所有数据块都被发送
            quint32 chunk_index = 0;
            while (sent_bytes < total_size) {
                size_t current_chunk_size = std::min(CHUNK_SIZE, total_size - sent_bytes);
                QByteArray chunk_data(data.data() + sent_bytes, current_chunk_size);
                
                // 更新统计信息
                chunk_index++;
                download_context->chunks_sent = chunk_index;
                
                // 发送反馈
                download_context->feedback->chunks_sent = download_context->chunks_sent;
                download_context->feedback->total_chunks = download_context->total_chunks;
                download_context->feedback->percentage = (download_context->total_chunks > 0) ? 
                    (static_cast<float>(download_context->chunks_sent) / download_context->total_chunks * 100.0f) : 0.0f;
                download_context->feedback->chunk_size = current_chunk_size;
                download_context->feedback->chunk_data.resize(current_chunk_size);
                memcpy(download_context->feedback->chunk_data.data(), chunk_data.data(), current_chunk_size);
                download_context->feedback->is_last_chunk = (chunk_index == download_context->total_chunks);
                
                // 发送地图数据块到ROS2 Action客户端
                LOG_DEBUG(" 准备发送反馈数据块 - 块索引: {}, 总块数: {}, 大小: {} 字节, 是否最后块: {}", 
                            download_context->chunks_sent, download_context->total_chunks, current_chunk_size, 
                            download_context->feedback->is_last_chunk);

                // 确保发送成功后再记录
                download_context->goal_handle->publish_feedback(download_context->feedback);
                
                sent_bytes += current_chunk_size;
            }
        }
        
        if (is_last_chunk) {
            // 下载完成
            Q_EMIT downloadFinished(download_context);
        }
    });
}


void MapDownloadActionServer::downloadMapStreaming(
    const QString& mapName,
    std::function<void(const QByteArray&, bool)> data_callback)
{
    LOG_INFO(" 开始流式下载地图 - 地图名称: {}", mapName.toStdString());
    
    // 构造请求JSON
    QJsonObject obj;
    obj["map_name"] = mapName;
    QJsonDocument doc(obj);
    QByteArray jsonData = doc.toJson(QJsonDocument::Compact);

    // 发送下载地图请求
    m_requestManager->sendRequest(m_confSocket, CMD_MAP_DOWNLOAD, jsonData,
        [this, mapName, data_callback](const QByteArray &resp) {
            if (resp.isEmpty()) {
                LOG_WARN(" 地图下载失败 - 地图名称: {}, 响应数据为空", mapName.toStdString());
                data_callback(QByteArray(), true); // 发送空数据和结束标志
                return;
            }

            // 直接将接收到的数据块转发给回调函数
            data_callback(resp, true);
            
            LOG_DEBUG(" 地图数据接收完成 - 大小: {} 字节", resp.size());
        }
    );
}

void MapDownloadActionServer::initializeConnections()
{
    LOG_INFO(" 初始化机器人连接");
    
    // 创建重连定时器
    m_reconnectTimer = new QTimer(this);
    m_reconnectTimer->setInterval(5000); // 5秒重连一次
    connect(m_reconnectTimer, &QTimer::timeout, this, &MapDownloadActionServer::reconnectTimerTimeout);
    
    // 尝试连接机器人
    connectToRobot();
    
    // 启动重连定时器
    m_reconnectTimer->start();
    LOG_INFO(" 重连定时器已启动，间隔: 5秒");
}

void MapDownloadActionServer::connectToRobot()
{
    LOG_DEBUG(" 尝试连接到机器人 - IP: {}, 配置端口: {}", 
                  m_robotIp.toStdString(), m_confPort);
    
    // 连接配置端口
    int result = m_confSocket->connectHost(m_robotIp, m_confPort);
    if (result == 0) {
        LOG_INFO(" 配置端口连接请求已发送");
    } else {
        LOG_WARN(" 配置端口连接请求发送失败或关闭了现有连接");
    }
}

bool MapDownloadActionServer::isConnected() const {
    QAbstractSocket::SocketState state = m_confSocket->tcpSocket()->state();
    LOG_DEBUG(" 检查连接状态: {}", static_cast<int>(state));
    return state == QAbstractSocket::ConnectedState;
}

void MapDownloadActionServer::scheduleReconnect()
{
    if (m_reconnectAttempts >= MAX_RECONNECT_ATTEMPTS) {
        LOG_ERROR(" 达到最大重连次数 {}，停止重连", MAX_RECONNECT_ATTEMPTS);
        return;
    }
    
    // 指数退避算法 + 抖动
    int baseDelay = 1000; // 基础延迟1秒
    int maxDelay = 30000; // 最大延迟30秒
    int delay = std::min(baseDelay * (1 << m_reconnectAttempts), maxDelay);
    
    // 添加抖动 (±20%)
    int jitter = (rand() % (delay / 5)) - (delay / 10);
    delay += jitter;
    delay = std::max(1000, delay); // 最小1秒
    
    m_reconnectAttempts++;
    LOG_INFO(" 计划在 {} ms 后进行第 {} 次重连", delay, m_reconnectAttempts);
    
    QTimer::singleShot(delay, this, &MapDownloadActionServer::connectToRobot);
}

void MapDownloadActionServer::resetReconnectAttempts()
{
    m_reconnectAttempts = 0;
}

void MapDownloadActionServer::updateConnectionStatus()
{
    bool connected = (m_confSocket->tcpSocket()->state() == QAbstractSocket::ConnectedState);
    
    if (connected && !m_wasConnected) {
        LOG_INFO(" 连接已恢复");
        m_wasConnected = true;
        resetReconnectAttempts(); // 重置重连次数
    } else if (!connected && m_wasConnected) {
        LOG_WARN(" 连接断开，开始重连流程");
        m_wasConnected = false;
    }
}

void MapDownloadActionServer::reconnectTimerTimeout()
{
    LOG_DEBUG(" 重连定时器超时，检查连接状态");
    
    updateConnectionStatus();
    
    bool needReconnect = false;
    
    // 检查配置连接
    if (m_confSocket->tcpSocket()->state() != QAbstractSocket::ConnectedState) {
        LOG_DEBUG(" 配置连接断开，需要重连");
        needReconnect = true;
    }
    
    // 如果需要重连，则使用改进的重连机制
    if (needReconnect) {
        LOG_INFO(" 检测到连接断开，尝试重新连接");
        scheduleReconnect();
    }
}

void MapDownloadActionServer::handleSocketStateChanged(QAbstractSocket::SocketState state)
{
    switch (state) {
        case QAbstractSocket::UnconnectedState:
            LOG_DEBUG(" 套接字状态: 未连接");
            break;
        case QAbstractSocket::HostLookupState:
            LOG_DEBUG(" 套接字状态: 正在查找主机");
            break;
        case QAbstractSocket::ConnectingState:
            LOG_DEBUG(" 套接字状态: 正在连接");
            break;
        case QAbstractSocket::ConnectedState:
            LOG_INFO(" 套接字状态: 已连接");
            break;
        case QAbstractSocket::BoundState:
            LOG_DEBUG(" 套接字状态: 已绑定");
            break;
        case QAbstractSocket::ListeningState:
            LOG_DEBUG(" 套接字状态: 监听中");
            break;
        case QAbstractSocket::ClosingState:
            LOG_DEBUG(" 套接字状态: 正在关闭");
            break;
    }
}

void MapDownloadActionServer::handleSocketError(QAbstractSocket::SocketError error)
{
    LOG_ERROR(" 套接字错误: {} - 检测到连接问题，计划重新连接", static_cast<int>(error));
    scheduleReconnect(); // 使用改进的重连机制
}

int main(int argc, char *argv[])
{
    // 初始化全局日志记录器
    GlobalLogger::getInstance().initialize("log", "RMSClient_map_download", 5);
    
    // 初始化Qt应用
    QCoreApplication app(argc, argv);
    
    rclcpp::init(argc, argv);
    auto node = std::make_shared<MapDownloadActionServer>();
    
    // 使用单线程执行器处理ROS 2回调
    rclcpp::executors::SingleThreadedExecutor executor;
    executor.add_node(node);
    
    // 创建一个QTimer来定期调用executor的spin_some
    QTimer timer;
    timer.setInterval(10); // 10ms
    QObject::connect(&timer, &QTimer::timeout, [&executor]() {
        executor.spin_some();
    });
    timer.start();
    
    // Qt事件循环在主线程中运行，同时定期处理ROS 2回调
    int result = app.exec();
    
    rclcpp::shutdown();
    return result;
}

#include "map_download_action_server.moc"